// Matrices.cpp: implementation of various special matrix classes.
// Part of KenTools library.
// Written by Kwan-Yee Kenneth Wong 2000.
//
//////////////////////////////////////////////////////////////////////


//#include "stdafx.h"
#include "Matrices.h"
#include <math.h>

//////////////////////////////////////////////////////////////////////
// class CMatrixB for B-Spline
//////////////////////////////////////////////////////////////////////
CMatrixB::CMatrixB() : CMatrix<double>(4,4)
{
	(*this)(0,0) = -1; (*this)(0,1) =  3; (*this)(0,2) = -3; (*this)(0,3) =  1;
	(*this)(1,0) =  3; (*this)(1,1) = -6; (*this)(1,2) =  3; (*this)(1,3) =  0;
	(*this)(2,0) = -3; (*this)(2,1) =  0; (*this)(2,2) =  3; (*this)(2,3) =  0;
	(*this)(3,0) =  1; (*this)(3,1) =  4; (*this)(3,2) =  1; (*this)(3,3) =  0;
	(*this) /= 6;
}

CMatrixB::~CMatrixB()
{

}

//////////////////////////////////////////////////////////////////////
// class CMatrixB_ for B-Spline
//////////////////////////////////////////////////////////////////////
CMatrixB_::CMatrixB_() : CMatrix<double>(3,4)
{
	(*this)(0,0) = -1; (*this)(0,1) =  3; (*this)(0,2) = -3; (*this)(0,3) =  1;
	(*this)(1,0) =  2; (*this)(1,1) = -4; (*this)(1,2) =  2; (*this)(1,3) =  0;
	(*this)(2,0) = -1; (*this)(2,1) =  0; (*this)(2,2) =  1; (*this)(2,3) =  0;
	(*this) /= 2;
}

CMatrixB_::~CMatrixB_()
{

}


///////////////////////////////////////
// class CVector for 2D or 3D vector
///////////////////////////////////////
//CVector::CVector(int nDim) : CMatrix<double>(nDim,1,0)
//{
//	
//}
//
//CVector::CVector(CVector& vec) : CMatrix<double>(vec.Size(), 1)
//{
//	if (SetSize(vec.Size(), 1))
//	{
//		memcpy(m_pData, vec.Buffer(), sizeof(double)*m_nRows);
//	}
//
//	(*this).Set(0,0,vec);
//}
//
//// destructor
//CVector::~CVector()
//{
//
//
//}
//
//const CVector& CVector::operator  = (const CVector& vec)
//{
//	if (vec.Size() == Size() || SetSize(vec.Size(), 1))
//	{
//		memcpy(m_pData, vec.Buffer(), sizeof(double)*m_nRows);
//	}
//	
//	return (*this);
//}
//
//CVector CVector::Homogeneous(double fLastDim)
//{
//	CVector result(m_nRows+1);
//	for (int i = 0; i < m_nRows; i++)
//		result(i) = (*this)(i);
//	result(m_nRows) = fLastDim;
//
//	return result;
//}


//////////////////////////////////////////////////////////////////////
// class CMatrixP for Camera Projection Matrix
//////////////////////////////////////////////////////////////////////
CMatrixP::CMatrixP() : CMatrix<double>(3,4)
{
	// initialize as [I 0]
	Diag(1.0); 
}

CMatrixP::CMatrixP(const CMatrixP& P) : CMatrix<double>(P)
{

}

CMatrixP::CMatrixP(const CMatrix<double>& P) : CMatrix<double>(P)
{

}

CMatrixP::~CMatrixP()
{

}

bool CMatrixP::GetViewDir(double& x, double& y, double& z) const
{
	if (isValid())
	{
		// V = P(2,0:2)
		x = (*this)(2,0);
		y = (*this)(2,1);
		z = (*this)(2,2);

		double norm = sqrt(x*x + y*y + z*z);
		x /= norm;
		y /= norm;
		z /= norm;

		return true;
	}
	else
	{
		return false;
	}
}

bool CMatrixP::GetViewDir(double V[3]) const
{
	if (isValid())
	{
		// V = P(2,0:2)
		V[0] = (*this)(2,0);
		V[1] = (*this)(2,1);
		V[2] = (*this)(2,2);

		double norm = sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
		V[0] /= norm;
		V[1] /= norm;
		V[2] /= norm;

		return true;
	}
	else
	{
		return false;
	}
}

CMatrix<double> CMatrixP::GetViewDir() const
{
	CMatrix<double> V;

	if (isValid())
	{
		// V = P(2,0:2)
		V = (*this)(2,2,0,2);

		if (V.isValid())
		{
			V /= V.GetVecNorm();
			V.SetSize(3);	// make it 3x1 instead of 1x3
		}
	}

	return V;
}

bool CMatrixP::GetCamCen(double& x, double& y, double& z) const
{
	CMatrix<double> Cm = GetCamCen();

	if (Cm.isValid())
	{
		x = Cm(0);
		y = Cm(1);
		z = Cm(2);

		return true;
	}
	else
	{
		return false;
	}
}

bool CMatrixP::GetCamCen(double C[3]) const
{
	CMatrix<double> Cm = GetCamCen();

	if (Cm.isValid())
	{
		C[0] = Cm(0);
		C[1] = Cm(1);
		C[2] = Cm(2);

		return true;
	}
	else
	{
		return false;
	}
}

CMatrix<double> CMatrixP::GetCamCen() const
{
	CMatrix<double> C;

	if (isValid())
	{
		// C = -P^(-1)*p, where [P p] is the camera matrix
		C = (*this)(0,2,0,2).Inverse() * GetCol(3);
		C *= -1.0;
	}

	return C;
}

bool CMatrixP::Calibrate(const CMatrix<double>& X, const CMatrix<double>& x)
{
	// check for correct sizes
	if (X.Rows()!=3 || x.Rows()!=2 || X.Cols()<6 || X.Cols()!=x.Cols())
	{
		return false;
	}

	CMatrix<double> A, U, D, V;

	// allocate memory
	if (!SetSize(3,4) || !A.SetSize(2*X.Cols(),12) ||
		!U.SetSize(2*X.Cols(),12) || !D.SetSize(12) || !V.SetSize(12,12))
	{
		CleanUp();
		return false;
	}

	A.SetZeros();

	for(int i=0; i<X.Cols(); i++)
	{
		A(2*i,0)  = X(0,i);
		A(2*i,1)  = X(1,i);
		A(2*i,2)  = X(2,i);
		A(2*i,3)  = 1.0;
		A(2*i,8)  = -x(0,i)*X(0,i);
		A(2*i,9)  = -x(0,i)*X(1,i);
		A(2*i,10) = -x(0,i)*X(2,i);
		A(2*i,11) = -x(0,i);

		A(2*i+1,4)  = X(0,i);
		A(2*i+1,5)  = X(1,i);
		A(2*i+1,6)  = X(2,i);
		A(2*i+1,7)  = 1.0;
		A(2*i+1,8)  = -x(1,i)*X(0,i);
		A(2*i+1,9)  = -x(1,i)*X(1,i);
		A(2*i+1,10) = -x(1,i)*X(2,i);
		A(2*i+1,11) = -x(1,i);
	}

	if (!A.SVD2(U, D, V))
	{
		CleanUp();
		return false;
	}

	for(int j=0; j<12; j++)
	{
		(*this)(j) = V(j,11);
	}

	return true;
}

bool CMatrixP::SetKRt(const CMatrix<double>& K, const CMatrix<double>& R, const CMatrix<double>& t)
{
	if (SetSize(3,4))
	{
		CMatrix<double> KR = K*R;
		CMatrix<double> Kt = K*t;

		if (KR.Rows()!=3 || KR.Cols()!=3 || Kt.Rows()!=3 || Kt.Cols()!=1)
		{
			return false;
		}
		else
		{
			return (Set(0,0, KR) && SetCol(3, Kt));
		}
	}
	else
	{
		return false;
	}
}

bool CMatrixP::Decompose(CMatrix<double>& K, CMatrix<double>& R, CMatrix<double>& T) const
{
	if (!isValid())
	{
		K.CleanUp();
		R.CleanUp();
		T.CleanUp();

		return false;
	}

	((*this)(0,2,0,2).Transpose()).QR2(R, K);

	K = K.Transpose();
	R = R.Transpose();

	// ensure the K matrix is meanful
	if (K.isValid() && R.isValid())
	{
		int i;
	
		// focal length in x-direction should not be negative
		if (K(0,0)<0)
		{
			for(i=0; i<3; i++)
			{
				K(i,0) = -K(i,0);
				R(0,i) = -R(0,i);
			}
		}
		
		// focal length in y-direction should not be negative
		if (K(1,1)<0)
		{
			for(i=0; i<3; i++)
			{
				K(i,1) = -K(i,1);
				R(1,i) = -R(1,i);
			}
		}

		// commment out by jerry (this is a false assumption)
		// principal point should not be negative
		/*if (K(0,2)<0)
		{
			for(i=0; i<3; i++)
			{
				K(i,2) = -K(i,2);
				R(2,i) = -R(2,i);
			}
		}*/

		T = K.Inverse()*GetCol(3);
		K /= K(2,2);

		if (T.isValid())
		{
			return true;
		}
		else
		{
			K.CleanUp();
			R.CleanUp();
			T.CleanUp();

			return false;
		}
	}
	else
	{
		K.CleanUp();
		R.CleanUp();
		T.CleanUp();

		return false;
	}
}

bool CMatrixP::FormF(const CMatrixP& P, CMatrix<double>& F) const
{
	if (!isValid() || !P.isValid())
	{
		return false;
	}

	CMatrix<double> Paux = P(0,2,0,2)*((*this)(0,2,0,2).Inverse());
	F = (P.GetCol(3) - Paux*((*this).GetCol(3))).Tx()*Paux;
 
	if (F.isValid())
	{
		return true;
	}
	else
	{
		return false;

	}
}

bool CMatrixP::Project(const double X, const double Y, const double Z, double& u, double& v) const
{
	if (isValid())
	{
		u = (*this)(0,0)*X + (*this)(0,1)*Y + (*this)(0,2)*Z + (*this)(0,3);
		v = (*this)(1,0)*X + (*this)(1,1)*Y + (*this)(1,2)*Z + (*this)(1,3);

		double s = (*this)(2,0)*X + (*this)(2,1)*Y + (*this)(2,2)*Z + (*this)(2,3);

		u /= s;
		v /= s;

		return true;
	}
	else
	{
		return false;
	}
}

CMatrix<double> CMatrixP::Triangulate(const CMatrixP& P_, const CMatrix<double>& x, const CMatrix<double> x_) const
{
	CMatrix<double> X;

	Triangulate(P_, x, x_, X);

	return X;
}

bool CMatrixP::Triangulate(const CMatrixP& P_, const CMatrix<double>& x, const CMatrix<double> x_, CMatrix<double>& X) const
{
	CMatrix<double> A, b;

	if (!isValid() || !P_.isValid() || x.Len()!=2 || x_.Len()!=2 ||
		!A.SetSize(4,3) || !b.SetSize(4,1))
	{
		X.CleanUp();
		return false;
	}
	else
	{
		const CMatrixP& P = (*this);

		if (!A.SetRow(0, P(0,0,0,2)-x(0)*P(2,2,0,2)) ||
			!A.SetRow(1, P(1,1,0,2)-x(1)*P(2,2,0,2)) ||
			!A.SetRow(2, P_(0,0,0,2)-x_(0)*P_(2,2,0,2)) ||
			!A.SetRow(3, P_(1,1,0,2)-x_(1)*P_(2,2,0,2)))
		{
			X.CleanUp();
			return false;
		}

		b(0) = x(0)*P(2,3)-P(0,3);
		b(1) = x(1)*P(2,3)-P(1,3);
		b(2) = x_(0)*P_(2,3)-P_(0,3);
		b(3) = x_(1)*P_(2,3)-P_(1,3);

		return (A.Solve(b, X));
	}
}

bool CMatrixP::Triangulate(const CMatrixP& P_, const double x[2], const double x_[2], double X[3]) const
{
	CMatrix<double> Mx, Mx_, MX;

	if (Mx.SetSize(2) && Mx_.SetSize(2))
	{
		Mx(0) = x[0];	Mx_(0) = x_[0];
		Mx(1) = x[1];	Mx_(1) = x_[1];

		if (Triangulate(P_, Mx, Mx_, MX))
		{
			X[0] = MX(0);
			X[1] = MX(1);
			X[2] = MX(2);

			return true;
		}
	}

	X[0] = X[1] = X[2] = 0;

	return false;
}

bool CMatrixP::BackProjection( const double x[2],CMatrix<double>& X ) const
{
	CMatrix<double> IM = (*this)(0,2,0,2).Inverse();
	CMatrix<double> hx(3);
	hx(0)=x[0]; hx(1)=x[1]; hx(2)=1.;
	X=IM*hx;
	return true;
}
//////////////////////////////////////////////////////////////////////
// class CMatrixR for rotation matrix
//////////////////////////////////////////////////////////////////////
CMatrixR::CMatrixR() : CMatrix<double>(3,3)
{
	// initialize as identity matrix
	Diag(1.0);
}

CMatrixR::CMatrixR(const CMatrixR& R) : CMatrix<double>(R)
{

}

CMatrixR::CMatrixR(const CMatrix<double>& R) : CMatrix<double>(R)
{

}

CMatrixR::CMatrixR( const double& Theta, const double N[3] ) : CMatrix<double>(3,3)
{
    SetRot(Theta,N);
}

CMatrixR::~CMatrixR()
{

}

bool CMatrixR::isValid() const
{
	if (m_nRows!=3 || m_nCols!=3)
	{
		return false;
	}

	double det = (*this)(0,0)*(*this)(1,1)*(*this)(2,2) 
			   + (*this)(0,1)*(*this)(1,2)*(*this)(2,0)
			   + (*this)(0,2)*(*this)(1,0)*(*this)(2,1)
			   - (*this)(0,0)*(*this)(1,2)*(*this)(2,1) 
			   - (*this)(0,1)*(*this)(1,0)*(*this)(2,2)
			   - (*this)(0,2)*(*this)(1,1)*(*this)(2,0);

	// check if det(R)==1
	if (fabs(det-1.0)>1e-12)
	{
		return false;
	}

	CMatrix<double> c0 = (*this).GetCol(0);
	CMatrix<double> c1 = (*this).GetCol(1);
	CMatrix<double> c2 = (*this).GetCol(2);	

	// check if it is orthonormal
	if (fabs(dot(c0,c0)-1.0) > 1e-12 ||
		fabs(dot(c1,c1)-1.0) > 1e-12 ||
		fabs(dot(c2,c2)-1.0) > 1e-12 ||
		fabs(dot(c0,c1)) > 1e-12 ||
		fabs(dot(c0,c2)) > 1e-12 ||
		fabs(dot(c1,c2)) > 1e-12)
	{
		return false;
	}

	return true;
}

CMatrixR& CMatrixR::SetRot(double theta, const CMatrix<double>& N)
{
	// ensure the matrix is of correct dimension
	if (!SetSize(3,3))
	{
		return *this;
	}

	double ct = cos(theta);
	double st = sin(theta);
	double ct_1 = 1.0 - ct;
	
	(*this)(0,0) = N(0)*N(0)*ct_1 + ct;
	(*this)(1,0) = N(1)*N(0)*ct_1 + N(2)*st;
	(*this)(2,0) = N(2)*N(0)*ct_1 - N(1)*st;

	(*this)(0,1) = N(0)*N(1)*ct_1 - N(2)*st;
	(*this)(1,1) = N(1)*N(1)*ct_1 + ct;
	(*this)(2,1) = N(2)*N(1)*ct_1 + N(0)*st;

	(*this)(0,2) = N(0)*N(2)*ct_1 + N(1)*st;
	(*this)(1,2) = N(1)*N(2)*ct_1 - N(0)*st;
	(*this)(2,2) = N(2)*N(2)*ct_1 + ct;

	return *this;
}

CMatrixR& CMatrixR::SetRot(double theta, const double N[3])
{
	// ensure the matrix is of correct dimension
	if (!SetSize(3,3))
	{
		return *this;
	}

	double ct = cos(theta);
	double st = sin(theta);
	double ct_1 = 1.0 - ct;
	
	(*this)(0,0) = N[0]*N[0]*ct_1 + ct;
	(*this)(1,0) = N[1]*N[0]*ct_1 + N[2]*st;
	(*this)(2,0) = N[2]*N[0]*ct_1 - N[1]*st;

	(*this)(0,1) = N[0]*N[1]*ct_1 - N[2]*st;
	(*this)(1,1) = N[1]*N[1]*ct_1 + ct;
	(*this)(2,1) = N[2]*N[1]*ct_1 + N[0]*st;

	(*this)(0,2) = N[0]*N[2]*ct_1 + N[1]*st;
	(*this)(1,2) = N[1]*N[2]*ct_1 - N[0]*st;
	(*this)(2,2) = N[2]*N[2]*ct_1 + ct;

	return *this;
}

CMatrixR& CMatrixR::SetRot(double Theta, double Az, double El)
{
	double N[3];
	double cos_El = cos(El);

    // this is the usual coordinate system with Z being elevation
    // using OpenGL, rotation is around Y becomes rotation around Z
	N[0] = cos_El*cos(Az);
	N[1] = cos_El*sin(Az);
	N[2] = sin(El);

	return SetRot(Theta, N);
}

bool CMatrixR::Decompose(double& Theta, CMatrix<double>& N) const
{
	if (!isValid())
	{
		Theta = 0;
		N.CleanUp();

		return false;
	}

	Theta = acos(((*this).Trace()-1.0)/2.0);

	if (!N.SetSize(3))
	{
		Theta = 0;
		return false;
	}

	double scale = -2.0*sin(Theta);

	N(0) = (*this)(1,2) - (*this)(2,1);
	N(1) = (*this)(2,0) - (*this)(0,2);
	N(2) = (*this)(0,1) - (*this)(1,0);

	N /= scale;

	return true;
}

bool CMatrixR::Decompose(double& Theta, double N[3]) const
{
	if (!isValid())
	{
		Theta = N[0] = N[1] = N[2] = 0;

		return false;
	}

	Theta = acos(((*this).Trace()-1.0)/2.0);

	double scale = -2.0*sin(Theta);

	N[0] = ((*this)(1,2) - (*this)(2,1))/scale;
	N[1] = ((*this)(2,0) - (*this)(0,2))/scale;
	N[2] = ((*this)(0,1) - (*this)(1,0))/scale;

	return true;
}

bool CMatrixR::Decompose(double& Theta, double& Az, double& El) const
{
	if (!isValid())
	{
		Theta = Az = El = 0;

		return false;
	}

	Theta = acos(((*this).Trace()-1.0)/2.0);

	double scale = -2.0*sin(Theta);

	double x = ((*this)(1,2) - (*this)(2,1))/scale;
	double y = ((*this)(2,0) - (*this)(0,2))/scale;
	double z = ((*this)(0,1) - (*this)(1,0))/scale;
	
	Az = atan2(y, x);
	double xy_norm = sqrt(x*x + y*y);
	El = atan2(z, xy_norm);

	return true;
}

CMatrixR CMatrixR::InverseR() const
{
	CMatrixR result;

	if (isValid())
	{
		for(int r=0; r<3; r++)
		{
			for(int c=0; c<3; c++)
			{
				result(c,r) = (*this)(r,c);
			}
		}
	}

	return result;
}

//////////////////////////////////////////////////////////////////////
// class CMatrixE for essential matrix
//////////////////////////////////////////////////////////////////////
CMatrixE::CMatrixE() : CMatrix<double>(3,3)
{
	// initialize as identity matrix
	Diag(1.0);
}

CMatrixE::CMatrixE(const CMatrixE& E) : CMatrix<double>(E)
{

}

CMatrixE::CMatrixE(const CMatrix<double>& E) : CMatrix<double>(E)
{

}

CMatrixE::~CMatrixE()
{

}

bool CMatrixE::Decompose(CMatrix<double>& R, CMatrix<double>& t, bool bPos, int nIdx) const
{
	CMatrixR R1, R2;

	if (Decompose2(R1, R2, t, bPos, nIdx))
	{
		double theta1, N1[3];
		double theta2, N2[3];

		bool bOK1 = R1.Decompose(theta1, N1);
		bool bOK2 = R2.Decompose(theta2, N2);

		if (bOK1 && bOK2)
		{
			// assume the wrong decomposition gives angle close to 180
			if (fabs(180-theta1)>fabs(180-theta2))
			{
				R = R1;
			}
			else
			{
				R = R2;
			}
		}
		else if (bOK1 && !bOK2)
		{
			R = R1;
		}
		else if (!bOK1 && bOK2)
		{
			R = R2;
		}
		else
		{
			R.CleanUp();
			t.CleanUp();

			return false;
		}

		if (R.Rows()!=3 && R.Cols()!=3)
		{
			R.CleanUp();
			t.CleanUp();

			return false;
		}

		return true;
	}
	else
	{
		R.CleanUp();
		t.CleanUp();

		return false;
	}
}

bool CMatrixE::Decompose2(CMatrix<double>& R1, CMatrix<double>& R2, CMatrix<double>& t, bool bPos, int nIdx) const
{
	if (isValid() && R1.SetSize(3,3) && R2.SetSize(3,3) && t.SetSize(3))
	{
		CMatrix<double> U, D, V, E;
		if (!SVD2(U, D, V)) // [u,d,v] = svd(E);
		{
			R1.CleanUp();
			R2.CleanUp();
			t.CleanUp();

			return false;
		}
			
		if (!D.SetSize(3,3))
		{
			R1.CleanUp();
			R2.CleanUp();
			t.CleanUp();

			return false;
		}

		D.SetZeros();
		D(0,0) = D(1,1) = 1.0;

		E = U*D*V.Transpose();

		if (E.Rows()!=3 || E.Cols()!=3)
		{
			R1.CleanUp();
			R2.CleanUp();
			t.CleanUp();

			return false;
		}

		CMatrix<double> c0 = E.GetCol(0);
		CMatrix<double> c1 = E.GetCol(1);
		CMatrix<double> c2 = E.GetCol(2);

		t = cross(c0, c1);
		if (t.Len()!=3)
		{
			R1.CleanUp();
			R2.CleanUp();
			t.CleanUp();
	
			return false;
		}

		// normalize t and assume the 3rd element (z) of t must have +ve sign
		t /= t.GetVecNorm();
		if (bPos)
		{
			if (t(nIdx)<0)
			{
				t(0) = -t(0);
				t(1) = -t(1);
				t(2) = -t(2);
			}
		}
		else
		{
			if (t(nIdx)>0)
			{
				t(0) = -t(0);
				t(1) = -t(1);
				t(2) = -t(2);
			}
		}

		CMatrix<double> E_;
		if (!E_.SetSize(3,3) ||
			!E_.SetCol(0, cross(c1, c2)) ||
			!E_.SetCol(1, cross(c2, c0)) ||
			!E_.SetCol(2, cross(c0, c1)))
		{
			R1.CleanUp();
			R2.CleanUp();
			t.CleanUp();

			return false;
		}

		R1 = (E_-t.Tx()*E);
		R2 = (E_+t.Tx()*E);
		if (R1.Rows()!=3 || R1.Cols()!=3 ||
			R2.Rows()!=3 || R2.Cols()!=3)
		{
			R1.CleanUp();
			R2.CleanUp();
			t.CleanUp();

			return false;
		}

		return true;
	}
	else
	{
		R1.CleanUp();
		R2.CleanUp();
		t.CleanUp();

		return false;
	}
}
